package lego.jbv;

import lejos.nxt.*;
import lejos.util.Delay;
import lejos.nxt.addon.RCXRotationSensor;

class TractionControl extends Thread {
	private int leftPower = 0;
	private int rightPower = 0;

	private final static int forward = 1, backward = 2, stop = 3;

	private RCXRotationSensor rotation;

	private static NXTRegulatedMotor leftMotor = Motor.B;
	private static NXTRegulatedMotor rightMotor = Motor.A;

	private boolean paused = true;

	public synchronized void pause() {
		paused = true;
	}

	public synchronized void restart() {
		paused = false;
	}

	public TractionControl() {
		rotation = new RCXRotationSensor(SensorPort.S1);
		LCD.drawString("Hej", 0, 3);
	}

	public synchronized int getLeftPower() {
		return leftPower;
	}

	public synchronized void setLeftPower(int leftPower) {
		this.leftPower = leftPower;
	}

	public synchronized int getRightPower() {
		return rightPower;
	}

	public synchronized void setRightPower(int rightPower) {
		this.rightPower = rightPower;
	}

	public void run() {
		int thirdWheelRotationSpeed = 0;
		int mainWheelRotation = 0;

		double kp = 0.08;
		double ki = 0.00075;
		double kd = 0.2;
		double tp = 75;
		double integral = 0;
		double lastError = 0;
		double derivative = 0;
		double error = 0;
		double speed = 0;
		//LCD.clear();
		while (true) {

			if (paused) {
				this.yield();
			} else {

				thirdWheelRotationSpeed = rotation.getRotationSpeed();
				mainWheelRotation = leftMotor.getRotationSpeed();
				
				LCD.drawInt(thirdWheelRotationSpeed, 7, 0, 0);
				LCD.drawInt(mainWheelRotation, 7, 0, 1);
				error = (double) thirdWheelRotationSpeed - mainWheelRotation;

				integral += error;

				derivative = error - lastError;

				speed = kp * error + ki * integral + kd * derivative;
				MotorPort.A.controlMotor((int) (leftPower + speed), 1);
				MotorPort.B.controlMotor((int) (leftPower + speed), 1);
				LCD.drawInt((int) (leftPower + speed), 7, 0, 2);

				lastError = error;
			}

		}
	}
}

public class Car {
	// Commands for the motors
	private final static int forward = 1, backward = 2, stop = 3;

	private MotorPort leftMotor = MotorPort.B;
	private MotorPort rightMotor = MotorPort.A;
	private TractionControl tractionControl;

	public Car() {
		tractionControl = new TractionControl();
		tractionControl.start();
	}

	public void stop() {
		leftMotor.controlMotor(0, stop);
		rightMotor.controlMotor(0, stop);
	}

	public boolean isActive() {
		int firstCount = leftMotor.getTachoCount();
		Delay.msDelay(10);
		int secondCount = leftMotor.getTachoCount();

		if (0 < secondCount - firstCount)
			return true;

		firstCount = rightMotor.getTachoCount();
		Delay.msDelay(10);
		secondCount = rightMotor.getTachoCount();

		if (0 < secondCount - firstCount)
			return true;

		return false;
	}

	public int counter() {
		return ((leftMotor.getTachoCount() + rightMotor.getTachoCount()) / 2);
	}

	public synchronized void drive(int leftPower, int rightPower,
			boolean TCS) {
		/*if (TCS) {
			tractionControl.restart();
			leftMotor.controlMotor(0, forward);
			rightMotor.controlMotor(0, forward);
			tractionControl.setLeftPower(leftPower);
			tractionControl.setRightPower(rightPower);
		} else {
			tractionControl.pause();*/
			if (0 <= leftPower) {
				leftMotor.controlMotor(leftPower, forward);
			}

			else {
				leftMotor.controlMotor(-leftPower, backward);
			}

			if (0 <= rightPower) {
				rightMotor.controlMotor(rightPower, forward);
			}

			else {
				rightMotor.controlMotor(-rightPower, backward);
			}
		

	}

	public void drive(int power, boolean TCS) {
		drive(power, power, TCS);
	}

	public void rotate(int angle, int speed) {
		drive(90,-90, false);
		Delay.msDelay(300);
	}

}
